#include <math.h>
#include <stdio.h>
#include "radar.h"
#include "key.h"
#include "tower.h"
#include "hero.h"
#include "definitions.h"
#include "toolkits/includes/glut.h"

extern int capturedKeys[KEYS];
extern int keyIDOnRadar;
extern float heroPosition[3];
extern float keyList[KEYS][3];
extern float distance;
extern float towerList[TOWERS][4];
extern float castlePosition[3];

Radar::Radar() {
}

float Radar::distance_calc(float origin[3], float destiny[3]){
	float dist = 0;
	float x=0,y=0,z=0;

	x = destiny[0] - origin[0];
	y = destiny[1] - origin[1];
	z = destiny[2] - origin[2];
  

	dist = sqrt(x*x + y*y + z*z);
	//dist = sqrt(x*x + z*z);

	return dist;
}

float Radar::radar(){
	float dist = -1, tmp = -1;
	bool first = true;
	keyIDOnRadar = -1;
	for(int i=0; i<KEYS; i++) {
		if (capturedKeys[i] != 1) {
			tmp = distance_calc(heroPosition, keyList[i]);
			// first iteration (minimum comparison)
			if (first && tmp <= RADAR_RANGE) {
				first = false;
				dist = tmp;
				/*keyIDOnRadar = keyList[i].id;*/
				keyIDOnRadar = i;
			}
			// if the new distance is lesser than the previous 
			if (tmp < dist && tmp <= RADAR_RANGE) {
				dist = tmp;
				/*keyIDOnRadar = keyList[i].id;*/
				keyIDOnRadar = i;
			}
		}
	}
	return dist;
}


void Radar::captureKey(){
	distance = radar(); // returns the nearest key or nothing (-1)
	if (distance >= 0 && distance <= HEROVELOCITY)
			capturedKeys[keyIDOnRadar] = 1;
}

//bool Radar::checkPosition (float pos1[3], float pos2[3]) {
//	bool equal = false;
//	int i=0;
//
//	while (i<3 && !equal) {
//		if (pos1[i] == pos2[i]) equal = true;
//		i=2;
//	}
//	return equal;
//}
//
//bool Radar::comparePosition (int type, int id, float pos[3]) {
//	bool equal = false;
//	for (int i=0; i<KEYS && !equal; i++) {
//		if (type != 0 && id != i) {
//			equal = checkPosition(pos, keyList[i]);
//			//if (type == 1)printf("TOWER:\n\tX:%f\n\tY:%f\n\tZ:%f\nKEY:\n\tX:%f\n\tY:%f\n\tZ:%f\n\tEqual:%s\n\n",pos[0],pos[1],pos[2],keyList[i].pos[0],keyList[i].pos[1],keyList[i].pos[2], equal?"true":"false");
//		}
//	}
//	
//	for (int j=0; j<TOWERS && !equal; j++) {
//		if (type != 1 && id != j) 
//			equal = checkPosition(pos, towerList[j]);
//	}
//	
//	if (!equal)
//		equal = checkPosition(pos, castlePosition);
//	
//	return equal;
//}
